#include <iostream>
#include <string>
#include <opencv2/opencv.hpp>

void calcRotation(const cv::Point &centerPoint, cv::Mat &invRot, int panoWidth, int panoHeight)
{
	//the angle that need to rotate to vertical with x axis
	float _anglex = centerPoint.x * 2 * M_PI / panoWidth;
	float _angley = M_PI / 2 - centerPoint.y * M_PI / panoHeight;

	cv::Mat Rotz = cv::Mat::eye(3, 3, CV_32F);
	cv::Mat Roty = cv::Mat::eye(3, 3, CV_32F);
	cv::Mat Rot = cv::Mat::eye(3, 3, CV_32F);

	Rotz.at<float>(0, 0) = cos(_anglex);
	Rotz.at<float>(0, 1) = sin(_anglex);
	Rotz.at<float>(1, 0) = -sin(_anglex);
	Rotz.at<float>(1, 1) = cos(_anglex);
	Roty.at<float>(0, 0) = cos(_angley);
	Roty.at<float>(0, 2) = sin(_angley);
	Roty.at<float>(2, 0) = -sin(_angley);
	Roty.at<float>(2, 2) = cos(_angley);
	Rot = Roty*Rotz;
	
	invRot = Rot.t();
}

void cutPlane(const cv::Mat pano, cv::Mat& plane, const cv::Point& centerPoint, float fov, cv::Size outSize)
{
	int outWidth = outSize.width;
	int outHeight = outSize.height;

	int panoWidth = pano.cols;
	int panoHeight = pano.rows;
	int channels = pano.channels();
	if (plane.empty())  plane = cv::Mat::zeros(outSize, pano.type());

	//normalized focal
	float f = outWidth / (2 * tan(fov / 2));


	cv::Mat Rot;
	calcRotation(centerPoint, Rot, panoWidth, panoHeight);
	

	uchar* panoData = (uchar*)pano.data;
	uchar* imgData = (uchar*)plane.data;
	float *pRot = (float*)Rot.data;
	for (int i = 0; i < outHeight; ++i)
	{
		float _z = i - outHeight / 2;
		for (int j = 0; j < outWidth; ++j)
		{
			float _x = f;
			float _y = j - outWidth / 2;
			float x = pRot[0] * _x + pRot[1] * _y + pRot[2] * _z;
			float y = pRot[3] * _x + pRot[4] * _y + pRot[5] * _z;
			float z = pRot[6] * _x + pRot[7] * _y + pRot[8] * _z;
			float theta = acos(z / sqrt(x*x + y*y + z*z));
			float fi = acos(x / sqrt(x*x + y*y));
			if (y < 0)
			{
				fi = 2 * M_PI - fi;
			}
			int i2 = theta * panoHeight / M_PI;
			int j2 = fi * panoWidth / (2 * M_PI);
			if (i2 < 0 || i2 >= panoHeight || j2 < 0 || j2 >= panoWidth)
			{
				continue;
			}
			for (int k = 0; k < channels; ++k)
			{
				imgData[((outHeight - 1 - i)*outWidth + j)*channels + k] = panoData[(i2*panoWidth + j2)*channels + k];
			}
		}
	}
}


void main()
{
	cv::Mat panoImg = cv::imread("data/pano.jpg", 1);


	//cut a plane
	cv::Mat planeImg;
	cv::Size outSize(1000, 1000);
	float FOV = 75 * M_PI / 180;

	cv::VideoWriter writer("data/plane.mp4", CV_FOURCC('D', 'I', 'V', 'X'), 30, outSize, 1);

	for (int i = 0; i < panoImg.cols; i +=10)
	{
		cv::Point center(i, panoImg.rows*0.5);
		cutPlane(panoImg, planeImg, center, FOV, outSize);

		cv::namedWindow("show", 0);
		cv::imshow("show", planeImg);
		cv::waitKey(1);

		writer << planeImg;
	}
	writer.release();

	cv::destroyAllWindows();

}